;
; Copyright (c) Microsoft Corporation.  All rights reserved.
;
;
; Use of this sample source code is subject to the terms of the Microsoft
; license agreement under which you licensed this sample source code. If
; you did not accept the terms of the license agreement, you are not
; authorized to use this sample source code. For the terms of the license,
; please see the license agreement between you and Microsoft or, if applicable,
; see the LICENSE.RTF on your install media or the root of your tools installation.
; THE SAMPLE SOURCE CODE IS PROVIDED "AS IS", WITH NO WARRANTIES OR INDEMNITIES.
;
;
; (C) Copyright 2006 Marvell International Ltd.
; All Rights Reserved
;

    INCLUDE kxarm.h
    INCLUDE monahans.inc
    INCLUDE littleton.inc
    INCLUDE image_cfg.inc
    
    INCLUDE monahans_macros.inc

    INCLUDE xlli_Littleton_defs.inc

	; D2
    INCLUDE xlli_Monahans_defs.inc
   
    
    TEXTAREA

    IMPORT	main
    IMPORT OALVAtoPA
    IMPORT OALPAtoVA
    IMPORT XScaleFlushICache
    IMPORT XScaleFlushDCache
    
    ; XLLI/XLLP imports
    IMPORT xlli_GPIO_init
    IMPORT xlli_Dmem_initP1
    IMPORT xlli_Dmem_initP2
    IMPORT xlli_MFPR_init
    IMPORT xlli_Smem_init
    IMPORT xlli_set_clocks

    
    ; Included within the text section in order that a relative offset can be
    ; computed in the code below.
    ;
    INCLUDE oemaddrtab_cfg.inc

;-------------------------------------------------------------------------------
;
; OALStartUp: OEM bootloader startup code.  This routine will:
;
; * Copy the image to RAM if it's not already running there.
;
; * Set up the MMU and Dcache for the bootloader.
;
; * Initialize the first-level page table based up the contents
;   of the MemoryMap array and enable the MMU and caches.
;
; Inputs: None.
; 
; On return: N/A.
;
; Register used:
;
;-------------------------------------------------------------------------------
;
    ALIGN
    
    LEAF_ENTRY OALStartUp

    ldr r0, =IMAGE_BOOT_STACK_RAM_PA_START	  ;  needs rework for xip.  Also, is plat-specific so needs to be addressed.
    mov sp, r0		  

    ; Initialize the hex LEDs on the Zylonite board.  These use GPIOs which were
    ; configured in the OALXScaleSetFrequencies callback during clock initialization.
    ;
    bl      Init_HexLEDs

    ; Copy the bootloader image from flash to RAM FOR NOR BOOTS ONLY. The image is configured
    ; to run in RAM, but is stored in flash.  Absolute address references
    ; should be avoided until the image has been relocated and the MMU enabled.
    ;
    ; **For NAND based boots, we will be paging this module into either IM or DDR before this gets called.
    ;
    ; May check to see if the PC is already in RAM.
    ;
    ; NOTE: The destination (RAM) address must match the address in the
    ; bootloader's .bib file.  The latter dictates the code fix-up addresses.
    ;
    
   IF :DEF: FLASH_NOR    
    ldr     r8, =IMAGE_BOOT_BLDRIMAGE_FLASH_PA_START ; Bootloader is stored at the base of the boot flash.
                                                   ; Bootloader is fixed up to run in SDRAM (this value
                                                   ; must match the NK start address in the .bib file).
    ldr     r1, =IMAGE_BOOT_BLDRIMAGE_RAM_PA_START
    ldr     r2, =(IMAGE_BOOT_BLDRIMAGE_RAM_SIZE / 16)    ; Bootloader image length (this must be <= the NK
                                                   ; length in the .bib file).  We are block-copying 
                                                   ; 16-bytes per iteration.
                                                   
    ; Do 4x32-bit block copies from flash->RAM (corrupts r4-r7).
    ;
10  ldmia   r8!, {r4-r7}        ; Loads from flash (post increment).
    stmia   r1!, {r4-r7}        ; Stores to RAM (post increment).
    subs    r2, r2, #1          ;
    bne     %B10                ; Done?

    ; Verify that the copy succeeded by comparing the flash and RAM contents.
    ;
    ldr     r0, =IMAGE_BOOT_BLDRIMAGE_RAM_SIZE
    ldr     r1, =IMAGE_BOOT_BLDRIMAGE_RAM_PA_START
    ldr     r2, =IMAGE_BOOT_BLDRIMAGE_FLASH_PA_START

VERIFY_LOOP
    ldr     r3, [r1], #4        ; Read longword from DRAM.
    ldr     r4, [r2], #4        ; Read longword from flash.
    cmp     r3, r4              ; Compare.
    bne     VERIFY_FAILURE      ; Not the same? Fail.
    subs    r0, r0, #4          ;
    bne     VERIFY_LOOP         ; Continue?
    b       VERIFY_DONE         ; Done (success).

VERIFY_FAILURE
STALL1
    b       STALL1              ; Spin forever.

VERIFY_DONE

    ; Now that we've copied ourselves to RAM, jump to the RAM image.  Use the "CodeInRAM" label
    ; to determine the RAM-based code address to which we should jump.
    ;
    add     r2, pc, #CODEINRAM-(.+8)          ; Calculate the relative offset to the 'CodeInRAM' label.
    ldr     r1, =IMAGE_BOOT_BLDRIMAGE_RAM_PA_START    ; Get the RAM address to which we copied ourself.
    add     r1, r1, r2                        ; Calculate the RAM address of the 'CodeInRAM' label.
    mov     pc, r1                            ;
    nop
    nop
    nop

CODEINRAM

    ; We're now running out of RAM.
    ; 

    ; Now that we're running out of RAM, construct the first-level Section descriptors
    ; to create 1MB mapped regions from the addresses defined in the OEMAddressTable.
    ; This will allow us to enable the MMU and use a virtual address space that matches
    ; the mapping used by the OS image.
    ;
    
  ENDIF ;IF :DEF: FLASH_NOR


    ;   Set up the Platform GPIOs
    ;
    bl      xlli_GPIO_init          ; Init the GPIO pins for use
    ldr     r0,     =xlli_MFPR_PHYSICAL_BASE; MFPR base address	; paramterize this for use in phy and virtual
    bl      xlli_MFPR_init          ; Init the MFPRs (Multi Function Pin Registers)

    ;
    ; DMEMC init, voltage change have been moved to MOBM. 
    ;
    ;
    ; hardcode MFPR for the ffuart
    ;
    ldr     r0, =xlli_MFPR_PHYSICAL_BASE
    ;ldr     r1, =0x0000A042		; 
    ;ldr     r1, =0x000000C2		;  power mod
    ;str	  r1, [r0, #0x438]	; gpio 41

    ldr	r1, =0xA041
    str	r1, [r0, #0x062C]   ; gpio 110, STRXD
    
    ;ldr	  r1, =0x0000A042		;
    ;ldr	  r1, =0x000000C2		;  power mod: txd
    ;str	  r1, [r0, #0x43C]	; gpio 42
    ldr	r1, =0xC041
    str	r1, [r0, #0x0628]	; gpio 109, STTXD
   
    ;
    ; Initialize the FFUART for debug messages.  Assumes xlli_GPIO_Init and xlli_MFPR_init have already run and
    ;	configured the pins for at least ffuart txd/rxd.
    ;
    ldr		r0, =MONAHANS_BASE_REG_PA_SIRUART    
    InitFFUART r0, r1, r2

    ; sign on string
    add		 r1, pc, #SIGNON-(.+8)
    PrintStr r0, r1, r2	; FBA, *str, gp

    add         r1, pc, #BOOTFREQ-(.+8)
    PrintStr r0, r1, r2	; FBA, *str, gp
    
    ldr         r0, =xlli_CLKREGS_PHYSICAL_BASE
    ldr         r1, [r0, #xlli_ACSR_offset]
    ldr		 r0, =MONAHANS_BASE_REG_PA_SIRUART
    PrintReg    r0, r1, r2,r3,r4
    
    
    ; Make sure sleep-mediated reset flag is cleared (won't be set if we're here 
    ; because of a POR, hardware, or watchdog reset.
    ;
    ;ldr     r1, =xlli_RCSR_SMR
    ;bic     r10, r10, r1

    ;
    ;Intialize Static Memory Controller for Ethernet.
    ;
    ldr r0, =xlli_STAMREGS_PHYSICAL_BASE
    bl xlli_Smem_init

    ;
    ; Override a few settings in xlli_Smem_init.
    ;   1) MSC1
    ;   2) CSADRCFG[3]
    ;   3) CSADRCFG[2]
    ;
    ldr r0, =xlli_STAMREGS_PHYSICAL_BASE
    ldr r1, =0x7FFC7FF8
    str r1, [r0, #xlli_MSC1_offset]
    ldr r1, [r0, #xlli_MSC1_offset]
    
    ldr r1, =0x003E080B         ; what we have been using.  need to investigate xlli values better.
    str r1, [r0, #xlli_CSADRCFG3_offset]
    ldr r1, [r0, #xlli_CSADRCFG3_offset]
    
    ldr r1, =0x00320919         ; What monahans.xdb also uses.
    str r1, [r0, #xlli_CSADRCFG2_offset]
    ldr r1, [r0, #xlli_CSADRCFG2_offset]
    
	;	  
    ; We'll create two different mappings from the addresses specified:
    ;     [8000 0000 --> 9FFF FFFF] = Cacheable, Bufferable (TEX=0, C=1, B=1)
    ;     [A000 0000 --> BFFF FFFF] = NonCacheable, nonBufferable (TEX=0, C=0, B=0)
    ;
BUILDTTB

    add     r11, pc, #g_oalAddressTable - (. + 8)    ; Pointer to OEMAddressTable.
    
    ; Set the TTB.
    ;
    ldr     r9, =IMAGE_BOOT_PAGETABLE_PA
    ldr     r0, =0xFFFFC000                   ;
    and     r9, r9, r0                        ; Mask off TTB[31:0] (must be 0's).
    mcr     p15, 0, r9, c2, c0, 0             ; Set the TTB.

    ; ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
    ; ~~~~~~~~~~ MAP CACHED and BUFFERED SECTION DESCRIPTORS ~~~~~~~~~~~~~~~~~~
    ; ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
    mov     r0, #0x0E                         ; Section (1MB) descriptor; (C=B=1: write-back, read-allocate).
    orr     r0, r0, #0x400                    ; Set AP.
20  mov     r1, r11                           ; Pointer to OEMAddressTable.

    ; Start Crunching through the OEMAddressTable[]:
    ;
    ; r2 temporarily holds OEMAddressTable[VA]
    ; r3 temporarily holds OEMAddressTable[PHY]
    ; r4 temporarily holds OEMAddressTable[#MB]
    ;
25  ldr     r2, [r1], #4                       ; Virtual (cached) address to map physical address to.
    ldr     r3, [r1], #4                       ; Physical address to map from.
    ldr     r4, [r1], #4                       ; Number of MB to map.

    cmp     r4, #0                             ; End of table?
    beq     %F29

    ; r2 holds the descriptor address (virtual address)
    ; r0 holds the actual section descriptor
    ;

    ; Create descriptor address.
    ;
    ldr     r6, =0xFFF00000
    and     r2, r2, r6             ; Only VA[31:20] are valid.
    orr     r2, r9, r2, LSR #18    ; Build the descriptor address:  r2 = (TTB[31:14} | VA[31:20] >> 18)

    ; Create the descriptor.
    ;
    ldr     r6, =0xFFF00000
    and     r3, r3, r6             ; Only PA[31:20] are valid for the descriptor and the rest will be static.
    orr     r0, r3, r0             ; Build the descriptor: r0 = (PA[31:20] | the rest of the descriptor)

    ; Store the descriptor at the proper (physical) address
    ;
28  str     r0, [r2], #4
    add     r0, r0, #0x00100000    ; Section descriptor for the next 1MB mapping (just add 1MB).
    sub     r4, r4, #1             ; Decrement number of MB left.
    cmp     r4, #0                 ; Done?
    bne     %B28                   ; No - map next MB.

    bic     r0, r0, #0xF0000000    ; Clear section base address field.
    bic     r0, r0, #0x0FF00000    ; Clear section base address field.
    b       %B25                   ; Get and process the next OEMAddressTable element.

    ; ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
    ; ~~~~~~~~~~ MAP UNCACHED and UNBUFFERED SECTION DESCRIPTORS ~~~~~~~~~~~~~~
    ; ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

29  tst     r0, #8                 ; Test for 'C' bit set (means we just used 
                                   ; above loop structure to map cached and buffered space).
    bic     r0, r0, #0x0C          ; Clear cached and buffered bits in the descriptor (clear C&B bits).
    add     r9, r9, #0x0800        ; Pointer to the first PTE for "unmapped uncached space" (0x2000 0000 + V_U_Adx).
    bne     %B20                   ; Repeat the descriptor setup for uncached space (map C=B=0 space).

ACTIVATEMMU
    
    ; The 1st Level Section Descriptors are setup. Initialize the MMU and turn it on.
    ;
    mov     r1, #1
    mcr     p15, 0, r1, c3, c0, 0   ; Set up access to domain 0.
    mcr     p15, 0, r0, c8, c7, 0   ; Flush the instruction and data TLBs.
    mcr     p15, 0, r1, c7, c10, 4  ; Drain the write and fill buffers.

    mov     r1, #0x78               ; Bits [6:3] must be written as 1's.
    orr     r1, r1, #0x1            ; Enable MMU.
    orr     r1, r1, #0x1000         ; Enable IC.
    orr     r1, r1, #0x0800         ; Enable BTB.
    orr     r1, r1, #0x4            ; Enable DC.
    ldr     r2, =VirtualStart       ; Get virtual address of 'VirtualStart' label.
    cmp     r2, #0                  ; Make sure no stall on "mov pc,r2" below.

    ;
    ; Enable the MMU.  There must be a flat-mapped descriptor in the table.
    mcr     p15, 0, r1, c1, c0, 0   ; MMU ON:  All memory accesses are now virtual.

    ; Jump to the virtual address of the 'VirtualStart' label.
    ;
    mov     pc, r2
    nop
    nop
    nop

    ; *************************************************************************
    ; *************************************************************************
    ; The MMU and caches are now enabled and we're running in a virtual
    ; address space.
    ;
    
    ALIGN
    
VirtualStart

    ;  Set up a supervisor mode stack. Blows away the previous stack (resets the sp to the top).
    ;
    ; NOTE: These values must match the OEMAddressTable and .bib file entries for
    ; the bootloader.
    ;
    ldr     sp, =IMAGE_BOOT_STACK_RAM_UA_START

    ; Jump to the C entrypoint.
    ;
    bl      main                              ; Jump to main.c::main(), never to return...

    
STALL2
    b      STALL2 

	ALIGN
SIGNON	DCB	0xA, 0xD, "Eboot Alive", 0xA, 0xD, 0
	
         ALIGN
BOOTFREQ	DCB	0xA, 0xD, "ACSR = ", 0xA, 0xD, 0



;-------------------------------------------------------------------------------
;
; Init_HexLEDs: Initializes the Zylonite board logic to enable the hex LEDs.
;
; Inputs: None.
; 
; On return: N/A.
;
; Register used: r0-r3
;
;-------------------------------------------------------------------------------
;
    ALIGN
Init_HexLEDs

  IF Interworking :LOR: Thumbing
    bx      lr
  ELSE
    mov     pc, lr                            ; Return to caller.
  ENDIF

                             

    ENTRY_END   OALStartUp
    
    
;-------------------------------------------------------------------------------
;
; void Launch(UINT32 pFunc): This function launches the program at pFunc (pFunc
;                            is a physical address).  The MMU is disabled just
;                            before jumping to specified address.
;
; Inputs: pFunc (r0) - Physical address of program to Launch.
; 
; On return: None - the launched program never returns.
;
; Register used:
;
;-------------------------------------------------------------------------------
;
    ALIGN
    LEAF_ENTRY Launch
   
    ; r3 now contains the physical launch address.
    ;
    mov     r3, r0

    ; Compute the physical address of the PhysicalStart tag.  We'll jump to this
    ; address once we've turned the MMU and caches off.
    ;
    stmdb   sp!, {r3}
    ldr     r0, =PhysicalStart
    bl      OALVAtoPA
    nop
    ldmia   sp!, {r3}
    
    ; r0 now contains the physical address of 'PhysicalStart'.
    ; r3 now contains the physical launch address.
   
    ; Next, we disable the MMU, and I&D caches.
    ;
    bl      XScaleFlushDCache
    bl      XScaleFlushICache
       
    mov     r1, #0x0078
    mcr     p15, 0, r1, c1, c0, 0

    ; Jump to 'PhysicalStart'.
    ;
    mov  pc, r0
    nop
    nop
    nop
    nop

PhysicalStart

    ; Flush the I&D TLBs.
    ;
    mcr     p15, 0, r2, c8, c7, 0   ; Flush the I&D TLBs

    ; Jump to the physical launch address.  This should never return...
    ;    

    ldr     r4, =0x40100000                 
    mov     r12, #0x33
    strb    r12, [r4, #0]
    
    mov     pc, r3
    nop
    nop
    nop
    nop
    nop
    nop
    nop
    nop
HUNG
	b HUNG

	LTORG	   
;-------------------------------------------------------------------------------
;-------------------------------------------------------------------------------
; This is for testing; will relocate this into IM
; Want to load at boot, and look for a magic number in the OS.  Re-load if not match.
;
; Input Params:  R0: address of data structure
;
; Exit requirements:
;   DDR:
;	1) clear DDR_HCAL[HCEN] to reset phase detector counters (DLC)
;	2) kick off an RCOMP cycle (xlli)
;	3) kick off HW DLC
;	4) (ensure RCOMP is done and codes updated if needed)
;	5) set MDCNFG[DMCEN] to re-enable the DMC
;-------------------------------------------------------------------------------
;-------------------------------------------------------------------------------
    ALIGN
    LEAF_ENTRY EnterLPM

	stmdb   sp!, {r0-r12, lr}   	; store off; especially lr

	;
	; Use a struct instead of registers, pass by reference.
	;
	; EnterD2(IPMD1D2Struct *);
	;
	;typedef struct __IPMD1D2Struct
	;{
	;	UINT32 ReloStartAdx;	#0
	;	UINT32 DMCBaseAdx;	#4
	;	UINT32 ReloSPAdx;	#8
	;	UINT32 CP14Val;		#C
	;} IPMD1D2Struct;
	;
	; Stash input parameters to upper registers
	;
	mov	r9,  sp			; stash DDR-resident SP  *DO NOT CORRUPT THIS REGISTER*

	ldr	r12, [r0, #0]		; stash start destination addy
	ldr	r11, [r0, #4]		; stash DMC base Addy
	ldr	r10, [r0, #8]  		; stash new RELO SP addy 

	; Put the CP14 value on the new (IM) stack for later
	;
	;  Stack layout:
	;	-------------------
	;	| <<unused slot>> |  TOP (<--r10)
	;	-------------------
	;	|   END_RELO lr   |  sp-4 (lr to jump back into DDR)
	;	-------------------
	;	|   CP14Val	  |  sp-8
	;	-------------------
	;
	ldr	r8,  [r0, #0xC]		; stash CP14 value (will stack it next)
	mov	sp,  r10		; use new stack for a sec...
	str	r8, [sp, #-8]		; store the value at the TOP-8 slot	
	mov	sp, r9			; restore the sp

	
	;
	; relocate loop
	; MH-L: take 5C02_8000 (end of stack reservation)
	;
	ldr	r0, [r0, #0]		; stash start destination addy
	ldr	r1, =START_RELO		; may need to deal with pc
	ldr	r2, =END_RELO
	sub	r3, r2, r1		; r3=r2-r1 == # bytes
	add	r4, r12, r3		; r4=r12(r0)+r3 == IM end address

RELO_LOOP
	ldr	r5, [r1], #+4		; corrupt r1.  Use stashed if needed.
	str	r5, [r0], #+4		; corrupt r0. Use stashed if needed.
	cmp	r0, r4
	blt	RELO_LOOP

	; relo done; need to ensure we dont incur a table walk to ddr
	; when exiting D2.  Will hang machine.
	;
	;  -) lock i and d TLBS.
	;  Assume using kernel-mode virtual address translation, using 1MB section descriptors.
	;

	; Lock I-TLB
	; Be paranoid; dont assume section alignment will save us.
	; (grab start, ~mid and end labels)
	;
	ldr	r0, =START_RELO
        mcr     p15, 0, r0, c8,  c5, 1		; invalidate i-TLB entry
	mcr	p15, 0, r0, c10, c4, 0

	ldr	r0, =DDR_REINIT
        mcr     p15, 0, r0, c8,  c5, 1		; invalidate i-TLB entry
	mcr	p15, 0, r0, c10, c4, 0

	ldr	r0, =END_RELO
        mcr     p15, 0, r0, c8,  c5, 1		; invalidate i-TLB entry
	mcr	p15, 0, r0, c10, c4, 0

	; Lock D-TLB
	;  This is a bit trickier; need to know what data we need.
	;  1) IM-resident literal pool data:  should be able to use same method as I-TLB for this, 
	;	since we have sprinkled several LTORGs within relocation boundaries.
	;  2) Memory-mapped registers:  these we just need to be aware of and load.  In general, a 1MB
	;	descriptor will cover an entire device; but need to really just load for each one, OR, try
	;	and load the entire peripheral area.
	;

	; Lock in literal pools
	;
	ldr	r0, =START_RELO
        mcr     p15, 0, r0, c8,  c6, 1		; invalidate d-TLB entry
	mcr 	p15, 0, r0, c10, c8, 0
	ldr	r0, =END_RELO
        mcr     p15, 0, r0, c8,  c6, 1		; invalidate d-TLB entry
	mcr 	p15, 0, r0, c10, c8, 0

	; Lock in mem-mapped translations for:
	;	1) DMC
        mcr     p15, 0, r11, c8,  c6, 1		; invalidate d-TLB entry
	mcr 	p15, 0, r11, c10, c8, 0

     IF :DEF: DEBUG_LED
	;	2) DEBUG_LED (requires oemaddrtab actually maps it)
	ldr	r0, =0xB7B00000
        mcr     p15, 0, r0, c8,  c6, 1		; invalidate d-TLB entry
	mcr 	p15, 0, r0, c10, c8, 0
     ENDIF

	; CPWAIT
	mrc p15, 0, r0, c2, c0, 0 		; arbitrary read of cp15
	mov r0, r0 				; wait for it
	sub pc, pc, #4 				; branch to next instruction
        
	; Branch to relocated code
	;
	ldr	lr, =END_RELO			; needs to be ddr addy
	mov	pc, r12	


	; !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
        ; !! Do not allow code to execute here.  Ensure this is never executed !!
	; !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
	LTORG				; dont pollute pools that get relocated


	;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
	; Marks the start of the relocated code and data
	;
	ALIGN 32
START_RELO
	;
	; Use new SP in IM
	; (now points to CP14 value)
	;
	mov	sp, r10

	; stack the lr on new stack (do no bl between these 2 instructions!!)
	; (will kill my cp14 val like this!)
	;
	stmdb   sp!, {lr}


  IF :DEF: DEBUG_LED
	; test: write some hex dbg action
	;
	ldr	r1, =0xB7B00000
	mov	r2, #0xA
	;mov	r2, r11
	mov	r2, r2, LSL #4
	;mov	r2, r11, LSR #16
	str	r2, [r1]
  ENDIF

	b ENTER_D2

	; !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
        ; !! Do not allow code to execute here.  Ensure this is never executed !!
	; !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
	LTORG				; ensure we get data pool within relocated area


ENTER_D2
	; enter S0D1/D2C2 state
	;mov r0, #3
	sub	sp, sp, #4
	ldmia	sp!, {r0}		; grab value from input structure
	b	D2_MODESWITCH

	ALIGN 32
D2_MODESWITCH
	mcr     p14, 0, r0, c7, c0, 0

	; wait for standby
	nop
	nop
	nop
	nop
	nop
	nop
	nop
	nop

	; There is some concern over the validity of the caches at this point
	; May need to clean and invalidate before entering the mode, and invalidate again after.

  IF :DEF: DEBUG_LED
	; test: write some hex dbg action
	;
	ldr	r1, =0xB7B00000
	mov	r2, #0xB
	mov	r2, r2, LSL #4
	str	r2, [r1]
  ENDIF


DDR_REINIT
	; r11 stores MDCNFG address
       ; disable DDR_HCAL[HCEN]
	ldr	r5, [r11, #0x60]		; DDR_HCAL offset 0x60
        bic     r5, r5, #0x80000000    ; clear HCEN
        str     r5, [r11, #0x60]
1
        ldr     r5, [r11, #0x60]
        tst     r5, #0x80000000
        bne     %b1

  IF :DEF: DEBUG_LED
	; test: write some hex dbg action
	;
	ldr	r1, =0xB7B00000
	mov	r2, #0x4
	;mov	r2, r11		; get LSW
	;mov	r2, r11, LSR #16
	mov	r2, r2, LSL #4
	str	r2, [r1]
  ENDIF


       ; initiate RCOMP[SWEAL]
        ldr     r6, [r11, #0x100]	; RCOMP offset 0x100
        orr     r6, r6, #0x80000000
        str     r6, [r11, #0x100]

  IF :DEF: DEBUG_LED
	; test: write some hex dbg action
	;
	ldr	r1, =0xB7B00000
	mov	r2, #0x5
	;mov	r2, r11		; get LSW
	;mov	r2, r11, LSR #16
	mov	r2, r2, LSL #4
	str	r2, [r1]
  ENDIF


       ; clear EDLP interrupt
        ldr     r7, =0xFFFFFFFF
        str     r7, [r11, #0x78]		; DMCISR offset 0x78

               ; set DMCIER[EDLP]
        ldr     r7, [r11, #0x70]
        orr     r7, r7, #0x20000000
        str     r7, [r11, #0x70]		; DMCIER offset 0x70

        ; set DDR_HCAL[HCEN]
        ; set DDR_HCAL[PROG]
        ; clear DDR_HCAL[HCRNG]
        ldr     r8, [r11, #0x60]		; hcal
        bic     r8, r8, #0x0000001F		; clear HCRNG
        orr     r8, r8, #0x09			; set HCRNG to 9
        str     r8, [r11, #0x60]

	; 2) RCOMP cycle (modified)
	;
	mov	r0, r11			; prime DMC base addy
	mov	r0, r0
	bl 	xlli_Rcomp		; should be pc-relative.

       ; enable MDCNFG[DMCEN]
        ldr     r5, [r11]		; MDCNFG offset 0x00
        orr     r5, r5, #0x40000000		; dmcen
        str     r5, [r11]
3
        ldr     r5, [r11]
        tst     r5, #0x40000000
        beq     %b3

       ; set DDR_HCAL[HCRNG]
        ldr     r6, [r11, #0x60]
        orr     r6, r6, #2
        str     r6, [r11, #0x60]



       ; clear interrupt
        ldr     r8, [r11, #0x70]		; DMCIER offset 0x70
        bic     r8, r8, #0x20000000
        str     r8, [r11, #0x70]

	;
	; *Return to DDR-resident Idle thread*
	; Restore the DDR-resident SP
	;
	ldmia   sp!, {lr}		; get the entry lr back
	mov sp, r9			; get the entry DDR-SP back
	mov pc, lr

	; !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
        ; !! Do not allow code to execute here.  Ensure this is never executed !!
	; !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
	LTORG				; ensure we get data pool within relocated area


;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;  H.E.L.P.eR Routines
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;       INPUT PARAMETER REQUIRED:
;               R0 = Dynamic Memory Control Reg Base Address
;
xlli_Rcomp      
        stmfd   sp!,    {r0-r12, lr}             	; Save r0-r7, & link on stack
        mov     r4,     r0      	 	 	; Copy Dynamic Memory Control Reg Base
;
; (stuff taken from initdmemP1)
; STEP 5: Perform Resistive Compensation calibration cycle (RCOMP)
;
        mov     r1,     #4             			; Set up Rcomp retry loop count
xlli_m3 
	ldr     r3,     =((xlli_RCOMP_RCRNG_value<<25) | xlli_RCOMP_REI)
        str     r3,     [r4, #xlli_RCOMP_offset]        ; Write inital values to RCOMP
        orr     r3,     r3,  #xlli_RCOMP_SWEVAL         ; Set bit to perform the RCOMP
        str     r3,     [r4, #xlli_RCOMP_offset]        ; Start the RCOMP evaluation
;
;       Wait for Rcomp evaluation cycle to complete - RCOMP[SWEVAL] clears.
;       A "time out" counter is in this loop - just in case the eval bit doesn't clear
;       so there is an exit path.
;
        mov     r0,     #0x10000                        ; Set up arbitrary time out counter
xlli_m4 
	ldr     r2,     [r4, #xlli_RCOMP_offset]        ; Fetch status
        subs    r0,     r0,     #1                      ; Decrement loop count
        beq     xlli_m9                                 ; Branch if timed out
        ands    r2,     r2,     #xlli_RCOMP_SWEVAL      ; Is the evaluation complete?
        bne     xlli_m4                                 ; No - loop until done.
	beq	CONTINUE_RCOMP

xlli_m9 
	subs    r1,     r1,     #1      		; Decrement re-try count
        bne     xlli_m3                 		; Try again if count is not zero
        ;b       xlli_m8                		; Otherwise - time to give up on this!!!
	; *if we time out, we have an issue.  We will just continue in this case.*

CONTINUE_RCOMP
        ldr     r2,     [r4, #xlli_DMCISR_offset]       ; Get results of the evaluation
        and     r6,     r2,     #0x003F8000             ; Extract NCODE
        mov     r6,     r6,     LSR #15                 ; and right justify
        and     r5,     r2,     #0x1FC00000             ; Extract PCODE
        mov     r5,     r5,     LSR #22                 ; and right justify

        ldr     r3,     =3480           		; load 0.348 * 10,000 into r3
        mul     r2,     r3,     r6      		; r2 = 3480(NCODE)
        ldr     r3,     =435            		; load 0.0435 * 10,000 into r3
        mul     r1,     r3,     r5      		; r1 = 435(PCODE)
        sub     r2,     r2,     r1      		; subtract the PCODE from the NCODE
        ldr     r3,     =(29600 + 5000) 		; load 2.96 * 10,000 + 5000 into r3
        sub     r0,     r2,     r3      		; r0 = PSLEW * 10,000 at this point
        ldr     r1,     =10000          		; Set up divisor
        bl      xlli_U32Divide          		; Divide!
        mov     r7,     r2              		; Save the calculated PSLEW value in r7
;
;       Do the NSLEW calculation. 5000 is added to NSLEW before it is divided
;       by 10,000 so the result is rounded to the closest whole number.
;
        ldr     r3,     =2500           		; load 0.250 * 10,000 into r3
        mul     r1,     r3,     r5      		; r1 = 250(PCODE)
        ldr     r3,     =1250           		; load 0.125 * 10,000 into r3
        mul     r2,     r3,     r6      		; r2 = 125(NCODE)
        sub     r1,     r1,     r2      		; Subtract the NCODE from the PCODE
        ldr     r3,     =(28800 + 5000) 		; load 2.88 * 10,000 + 5000 into r3
        add     r0,     r2,     r3      		; r0 = NSLEW * 10,000 at this point
        ldr     r1,     =10000          		; Set up divisor
        bl      xlli_U32Divide          		; Divide!
        mov     r8,     r2              		; Save the calculated NSLEW value in r8
;
;       At this point the registers we care about contain the following:
;
;               r4 = Dynamic Memory Control Reg Base Address
;               r5 = PCODE value
;               r6 = NCODE value
;               r7 = PSLEW value
;               r8 = NSLEW value
;
;       Next step is to combine the NCODE, PCODE, NSLEW, PSLEW values and program
;       all the pad registers to the same value.
;
        mov     r5,     r5,     LSL #24 ; Move the PCODE into position (LSB to bit 24)
        mov     r6,     r6,     LSL #16 ; Move the NCODE into position (LSB to bit 16)
        mov     r7,     r7,     LSL #8  ; Move the PSLEW into position (LSB to bit 8)
        orr     r8,     r8,     r7      ; OR the NSLEW and PSLEW values together
        orr     r8,     r8,     r6      ; OR in the NCODE value
        orr     r8,     r8,     r5      ; OR in the PCODE value
;
;       The PAD register data has been assembled into r3. Now the code needs
;       to send this data to the selected PAD registers.
;
        str     r8,     [r4, #xlli_PAD_MA_offset]       ; Load the pad registers
        str     r8,     [r4, #xlli_PAD_MDMSB_offset]    ;
        str     r8,     [r4, #xlli_PAD_MDLSB_offset]    ;
        str     r8,     [r4, #xlli_PAD_DMEM_offset]     ;
        str     r8,     [r4, #xlli_PAD_SDCLK_offset]    ;
        str     r8,     [r4, #xlli_PAD_SDCS_offset]     ;
        str     r8,     [r4, #xlli_PAD_SMEM_offset]     ;
        str     r8,     [r4, #xlli_PAD_SCLK_offset]     ;
;
;       Set the UPDATE bit in the Rcomp register
;
        ldr     r3,     [r4, #xlli_RCOMP_offset]        ; Fetch RCOMP register value
        orr     r3,     r3,     #xlli_RCOMP_UPDATE      ; Set the UPDATE bit
        str     r3,     [r4, #xlli_RCOMP_offset]        ; Write RCOMP register back       

        ldmfd   sp!,    {r0-r12, pc}             ; Return to caller via the stack

	; !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
        ; !! Do not allow code to execute here.  Ensure this is never executed !!
	; !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
	LTORG				; ensure we get data pool within relocated area


;*********************************************************************************
;
;       ******************
;       *                * 
;       * xlli_U32Divide * Subroutine
;       *                *
;       ******************
;
; This subroutine performs an unsigned 32 bit divide
;
; PARAMETER PASSING:
;
; r0  contains the dividend (preserved)
; r1  contains the divisor  (preserved)
; r2  will contain the result on subroutine exit
; r3  will contain the remainder on subroutine exit 
;
xlli_U32Divide  

        stmfd   sp!,    {r0, r1, r4, r5, lr}    ; Save used registers

        mov     r2,     #0      ; Clear result
        mov     r3,     #0      ; Clear remainder
        mov     r5,     #32     ; load r5 with loop count
        mrs     r4,     CPSR                    ; Get CPSR
        bic     r4,     r4,     #0x20000000     ; Clear carry bit
;
;       Main division loop begins here
;
xlli_D1 
	msr     CPSR_f,         r4      ; write flags back
        adcs    r2,     r2,     r2      ; Shift C bit into result
        adcs    r0,     r0,     r0      ; Rotate r0 left 1 bit through C bit
        adcs    r3,     r3,     r3      ; Rotate data left, into r3
        subs    r3,     r3,     r1      ; Subtract dividend
        blt     xlli_D2                 ; Branch if negative
        mrs     r4,     CPSR            ; Get CPSR
        orr     r4,     r4,     #0x20000000     ; Set carry bit
        msr     CPSR_f,         r4      ; write flags back
        b       xlli_D3                 ; Check loop count

xlli_D2 
	add     r3,     r3,     r1      ; Restore r3
        mrs     r4,     CPSR            ; Get CPSR
        bic     r4,     r4,     #0x20000000     ; Clear carry bit
        msr     CPSR_f,         r4      ; write flags back

xlli_D3 
	mrs     r4,     CPSR            ; Get CPSR
        subs    r5,     r5,     #1      ; decrement loop count
        bne     xlli_D1                 ; Loop until done
;
;       Shift the last bit into the result and return to caller
;
        msr     CPSR_f,         r4      ; write flags back
        adcs    r2,     r2,     r2      ; Shift C bit into result
        adcs    r0,     r0,     r0      ; Rotate r0 left 1 bit through C bit

        ldmfd   sp!,    {r0, r1, r4, r5, pc}    ; Return to caller vis the stack

	; !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
        ; !! Do not allow code to execute here.  Ensure this is never executed !!
	; !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
	LTORG				; ensure we get data pool within relocated area


;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
; Marks the end of the relocated code and data
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
END_RELO				; back in DDR
	ldmia   sp!, {r0-r12, lr}   
        mcr p15, 0, r0, c10, c4, 1	; unlock entire i-TLB
        mcr p15, 0, r0, c10, c8, 1	; unlock entire d-TLB

	; ! For stress test only; dont do this in OS !
	;mcr p15, 0, r0, c7, c6, 0	; invalidate d$ (force DDR fetches)
    ;    mcr p15, 0, r0, c7, c5, 0	; invalidate i$ and BTB (force DDR fetches)

	mov pc, lr			; back to caller
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
    END

    
